def _constrain(x,xmin,xmax):
    if x < xmin: return xmin
    elif x > xmax: return xmax
    else: return x

class PID():
    def __init__(self,kp,ki,kd,integ_constrain):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self._err_delta = 0.0
        self._err_integ = 0.0
        self._err_prev = 0.0
        self._integ_constrain = integ_constrain
    def forward(self,x,std):
        err = x-std
        # 计算反馈
        feedback = self.kp*err + self.ki*self._err_integ + self.kd * self._err_delta
        # 计算积分项，微分项和前项
        self._err_integ += err
        self._err_delta = err-self._err_prev
        self._err_prev = err
        self._err_integ = _constrain(self._err_integ,-self._integ_constrain,+self._integ_constrain)
        return feedback

